import { Vec3 } from "cc";

const tempVec1 = new Vec3()
const tempVec2 = new Vec3()
const tempVec3 = new Vec3()

export class MathUtil {
    //获取带方向的夹角
    static signAngle(from: Vec3, to: Vec3, axis: Vec3): number {
        const angle = Vec3.angle(from, to)
        let cross = new Vec3()
        Vec3.cross(cross, from, to)
        const sign = Math.sign(cross.x * axis.x + cross.y * axis.y + cross.z + axis.z)
        return sign * angle
    }

    //罗德里格旋转公式

    static rotateAround(out: Vec3, forward: Vec3, axis: Vec3, maxAngleDelta: number) {

        const cos = Math.cos(maxAngleDelta)
        const sin = Math.sin(maxAngleDelta)

        // v*cos
        Vec3.multiplyScalar(tempVec1, forward, cos)
        // u*v
        Vec3.cross(tempVec2, axis, forward)
        // v*cos+uxv*sin
        Vec3.scaleAndAdd(tempVec3, tempVec1, tempVec2, sin)

        const dot = Vec3.dot(axis, forward)
        // +(u*v*u*(1-cos))
        Vec3.scaleAndAdd(out, tempVec3, axis, dot * (1.0 - cos))
    }

    static rotateToWorld(out: Vec3, from: Vec3, to: Vec3, maxAngleDelta: number) {
        Vec3.cross(tempVec1, from, to)
        this.rotateAround(out, from, tempVec1, maxAngleDelta)
    }
}